#include <chrono>
#include <iostream>

#include "simulation/client/client.h"

using namespace simulation::client;

int main(int argc, char** argv) {
  Client client;

  Callbacks callbacks;
  callbacks.on_localization = [&](const LocalizationInfo& info) {
    std::cout << "on localization " << info.timestamp / 1e6 << std::endl;
    std::cout << info.cartesian_lla[0] << " " << info.cartesian_lla[1]
              << std::endl;
    std::cout << info.linear_vel_xyz[0] << " " << info.linear_vel_xyz[1] << " "
              << info.linear_vel_xyz[2] << std::endl;
    std::cout << info.linear_accel_xyz[0] << " " << info.linear_accel_xyz[1]
              << " " << info.linear_accel_xyz[2] << std::endl;
    std::cout << info.linear_accel_xyz[0] << " " << info.linear_accel_xyz[1]
              << " " << info.linear_accel_xyz[2] << std::endl;
  };

  callbacks.on_perception = [&](const PerceptionInfo& info) {
    std::cout << "on perception " << info.timestamp / 1e6 << std::endl;
  };

  callbacks.on_camera_images = [](const std::vector<Image>& images) {
    std::cout << "on image " << images[0].timestamp / 1e6 << std::endl;
  };

  auto start_t = std::chrono::steady_clock::now();

  callbacks.on_chassis = [&](const ChassisInfo& info) {
    VehicleControl vc;

    if (std::chrono::steady_clock::now() - start_t <= std::chrono::seconds(3)) {
      vc.throttle = 0.0;
      vc.steer = 0.0;
      vc.brake = 0.0;
    } else if (std::chrono::steady_clock::now() - start_t <=
               std::chrono::seconds(4)) {
      vc.throttle = 0.0;
      vc.steer = 0.0;
      vc.brake = 0.5;
    } else {
      vc.throttle = 0.5;
      vc.steer = 0.0;
      vc.brake = 0.0;
    }
    vc.hand_brake = false;
    vc.reverse = false;
    vc.manual_gear_shift = false;
    vc.gear = 1;

    client.SendControlCommand(vc);
  };

  auto r = client.Connect("172.16.34.76:17000");
  if (std::get<0>(r)) {
    r = client.Run("001", "hero", callbacks);
    if (std::get<0>(r)) {
      return 0;
    }
  }

  std::clog << "Client::Run() failed, error info:" << std::get<1>(r)
            << std::endl;

  return -1;
}
